/**
 * Copyright (c) 2024, https://www.zhz.com/
 * All rights reserved.
 * $Revision: 1.0 $
 * $Date: 2024/06/14 20:33:40 $
 * $Author: MSun $
 ******************************************************************************/

#include "zhz_img_convert.h"

namespace zhz {

  DriverCamera::DriverCamera(): nh_("~"),img_nh_(nh_){
    Init();
  }

  DriverCamera::~DriverCamera() {
    // cv::destroyWindow("view");
    ros::shutdown(); 
  }

  void DriverCamera::Run() {
    ros::Rate loop_rate(this->pub_rate_);
    while (ros::ok())
    {
      ros::spinOnce();
      loop_rate.sleep();
    }
    return;
  }

  void DriverCamera::Init() {
    // image_publisher_ = nh.advertise<AprilTagDetectionArray>("tag_detections", 1);
    // image_publisher_ = img_nh_.advertise( "/zhz/driver/image_raw", 1 );
    
    std::string left_topic, right_topic, camera_info_l, camera_info_r;
    ros::param::get("~left_topic", left_topic);
    ros::param::get("~right_topic", right_topic);
    ros::param::get("~camera_param", camera_param_);
    ros::param::get("~pub_rate", pub_rate_);
    ros::param::get("~use_right_camera", use_right_camera_);
    ros::param::get("~camera_info_l", camera_info_l);
    ros::param::get("~camera_info_r", camera_info_r);

    camera_info_pub_l_ = nh_.advertise<sensor_msgs::CameraInfo>(camera_info_l, 1);
    camera_info_pub_r_ = nh_.advertise<sensor_msgs::CameraInfo>(camera_info_r, 1);
    image_sub_l_ = img_nh_.subscribe(left_topic, 10, &DriverCamera::ImageProcess, this);
    image_pub_l_ = img_nh_.advertiseCamera(left_topic + "_camera_info", 1);
    GetCameraInfo(cam_info_l_);
    if (use_right_camera_) {
      image_sub_r_ = img_nh_.subscribe(right_topic, 10, &DriverCamera::ImageProcessR, this);
      image_pub_r_ = img_nh_.advertiseCamera(right_topic + "_camera_info", 1 );
      GetCameraInfo(cam_info_r_);
    }
  }

  void DriverCamera::GetCameraInfo(sensor_msgs::CameraInfo &cam){
    cv::FileStorage fs_reader;

    if (!camera_param_.empty()) {
        fs_reader.open(camera_param_, cv::FileStorage::READ);
        if (!fs_reader.isOpened()) {
          ROS_ERROR_STREAM("Failed to open camera parameters file: " << camera_param_);
          return; // or handle the error as appropriate
        }
    } else {
      ROS_ERROR("Camera parameter file path is empty.");
      return; // or handle the error as appropriate
    }

    fs_reader["CameraInstrincMatL"] >> camera_intrinsic_l;
    fs_reader["CameraExtrinsicMatL"] >> camera_extrinsic_l;
    fs_reader["DistCoeffL"] >> camera_distcoeff_l;
    fs_reader["CameraInstrincMatR"] >> camera_intrinsic_r;
    fs_reader["CameraExtrinsicMatR"] >> camera_extrinsic_r;
    fs_reader["DistCoeffR"] >> camera_distcoeff_r;
    fs_reader["ImageSize"] >> image_size_;
    fs_reader["RotMatL2R"] >> RotMatL2R;
    fs_reader["TransMatL2R"] >> TransMatL2R;
    fs_reader.release();

    image_width_ = image_size_.width;
    image_height_ = image_size_.height;

    auto& d = camera_distcoeff_l;
    auto& k = camera_intrinsic_l;
    std::vector<double> D{d.at<double>(0), d.at<double>(1), d.at<double>(2), d.at<double>(3), d.at<double>(4)};
    boost::array<double, 9> K = {k.at<double>(0), k.at<double>(1), k.at<double>(2),
                                k.at<double>(3), k.at<double>(4), k.at<double>(5),
                                k.at<double>(6), k.at<double>(7), k.at<double>(8)};
    
    cv::Mat p = getOptimalNewCameraMatrix(k, d, image_size_, 0);        // get rectified projection.
    boost::array<double, 12> P = {
            p.at<double>(0,0),p.at<double>(0,1),p.at<double>(0,2),p.at<double>(0,3),
            p.at<double>(1,0),p.at<double>(1,1),p.at<double>(1,2),p.at<double>(1,3),
            p.at<double>(2,0),p.at<double>(2,1),p.at<double>(2,2),p.at<double>(2,3)};
    boost::array<double, 9> r = {1, 0, 0, 0, 1, 0, 0, 0, 1};

    std::cout << "Proj: " << p << std::endl;
    cam.height = image_height_;
    cam.width = image_width_;
    cam.distortion_model = "plumb_bob";
    cam.D = D;
    cam.K = K;
    cam.P = P;
    cam.R = r;
    cam.binning_x = 0;
    cam.binning_y = 0;
    return;
}

  void DriverCamera::ImageProcess(const sensor_msgs::ImageConstPtr& msg) {
      cam_info_l_.header = msg->header;
      image_pub_l_.publish(*msg, cam_info_l_);
      camera_info_pub_l_.publish(cam_info_l_);
  }

  void DriverCamera::ImageProcessR(const sensor_msgs::ImageConstPtr& msg) {
      cam_info_r_.header = msg->header;
      image_pub_r_.publish(*msg, cam_info_r_);
  }
} // namespace zhz



